# -*- coding: UTF-8 -*-
#! /usr/bin/env python
import rospy
import actionlib
from moveaction.msg import *
from Class.FrankaPanda import FrankaPanda
from Class.Message import HOME_JOINT_7


class MyActionServer:
    def __init__(self):
        #SimpleActionServer(name, ActionSpec, execute_cb=None, auto_start=True)
        self.server = actionlib.SimpleActionServer("move_serve",FrankaMoveAction,self.cb,False)
        self.server.start()
        self.franka = FrankaPanda()
        rospy.loginfo("服务端启动")


    def cb(self,goal):
        rospy.loginfo("服务端处理请求:")
        #1.解析目标值
        
        x = goal.x
        y = goal.y 
        z = goal.z

        gripper = goal.gripper 
        movtion = goal.movtion 

        result = FrankaMoveResult()

        if movtion == 0:
            rospy.loginfo("Arm Moving:Home")
            self.franka.Go(HOME_JOINT_7)
            result.reslut = "SUCCESS"

        if gripper == 0:
            rospy.loginfo("Grasp Closing")
            self.franka.GraspClose()
            result.reslut = "SUCCESS"

        if gripper == 1:
            rospy.loginfo("Grasp Opening")
            self.franka.GraspOpen()
            result.reslut = "SUCCESS"

        if movtion == -1 and gripper == -1:

            rospy.loginfo("Arm Moveing:Angle")

            ################################
            
            calres,Joint = self.franka.MoveTo([x,y,z])

            if not calres:

                result.reslut = "ERROR:Bad Point"
                rospy.loginfo("ERROR:Bad Point")

            else:
                moveres = True
                try:
                    self.franka.Go(Joint)
                except EOFError as e:
                    rospy.loginfo(e)
                    result.reslut = "ERROR:Bad Move"
                    moveres = False

            if calres and moveres:
                result.reslut = "SUCESS"

            ##############################

        self.server.set_succeeded(result)
        rospy.loginfo("响应")
        
if __name__ == "__main__":
    rospy.init_node("action_server_p")
    server = MyActionServer()
    rospy.spin()
